#ifndef LEADERMPC_H
#define LEADERMPC_H

#include <vector>
#include <map>
#include <Eigen/Core>
#include <bits/stdc++.h>

using namespace std;
// 前向声明
class RobustSafetyMarginCalculator;

class Robust_BCI_MPC
{
    public:

        Robust_BCI_MPC();
        ~Robust_BCI_MPC();
        // Solve the model given an initial state and user' command.
        // Return the first actuatotions.
        vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd vec_cmd);
        // 新增：带不确定性的求解方法
        vector<double> SolveWithUncertainty(Eigen::VectorXd state, Eigen::VectorXd vec_cmd, 
                                          double bci_confidence, const Eigen::VectorXd& obstacle_pos);

        // predicted value
        vector<double> mpc_x;
        vector<double> mpc_y;
        vector<double> mpc_theta;

        double _mpc_totalcost;
        
        void LoadParams(const std::map<string, double> &params);
        // 设置不确定性参数 
        void SetUncertaintyParams(double confidence, double bound);
        void SetObstaclePosition(const Eigen::VectorXd& obstacle_pos);
        // 预测障碍物轨迹（简化：假设线性运动）
        std::vector<Eigen::VectorXd> predictObstacleTrajectory(const Eigen::VectorXd& current_obstacle_pos,
                                                            int steps) const;
        // 基于预测轨迹计算鲁棒安全距离
        // 基于预测轨迹计算鲁棒安全距离
        std::vector<double> computeTrajectoryBasedSafetyDistances(
            const Eigen::VectorXd& initial_state,
            const Eigen::VectorXd& obstacle_pos,
            double bci_confidence);
            
    private:
        // Parameters for mpc solver
        double _max_angvel, _max_speed, _bound_value;
        int _mpc_steps, _x_start, _y_start, _theta_start, _distance_start, _degree_start, _linvel_start, _angvel_start;
        std::map<string, double> _params;
        // 预测状态轨迹存储
        std::vector<Eigen::VectorXd> _predicted_states;
        std::vector<Eigen::VectorXd> _predicted_obstacles;

        // 新增：不确定性相关成员
        double _bci_confidence;
        double _uncertainty_bound;
        Eigen::VectorXd _current_obstacle_pos;
        RobustSafetyMarginCalculator* _robust_calculator;

        // 辅助方法声明
        std::vector<Eigen::VectorXd> generateNominalTrajectory(const Eigen::VectorXd& initial_state);
        
        // 预测状态轨迹存储
        std::vector<Eigen::VectorXd> _predicted_states;
        std::vector<Eigen::VectorXd> _predicted_obstacles;
    

};

#endif /* LEADERMPC_H */